#include <cerrno>
#include <cstdio>
#include <iostream>
#include <time.h>
#include <fstream>
#include <math.h>
#include "DNA_vec_types.h"

using namespace std;

#define PI 3.14159265358979323846

vec3f cube[8];
vec2f imageCube[8];
float rotz[3][3];
float focal;
float rot;

int main(int argc, char** argv)
{
	int i;
	//Init cube data
	cube[0].x=0; cube[0].y=0; cube[0].z=0;
	cube[1].x=1; cube[1].y=0; cube[1].z=0;
	cube[2].x=0; cube[2].y=1; cube[2].z=0;
	cube[3].x=1; cube[3].y=1; cube[3].z=0;
	cube[4].x=0; cube[4].y=0; cube[4].z=1;
	cube[5].x=1; cube[5].y=0; cube[5].z=1;
	cube[6].x=0; cube[6].y=1; cube[6].z=1;
	cube[7].x=1; cube[7].y=1; cube[7].z=1;

	//Get focal length
	cout << "Set Focal length\n";
	cin >> focal;
	//Get angle rotation
	cout << "Set z rotation\n";
	cin >> rot;
	//set rot to rad
	rot=rot*PI/180.0;
	//Set Transform matrix	
	rotz[0][0]=cos(rot);
	rotz[0][1]=0;
	rotz[0][2]=-sin(rot);
	
	rotz[1][0]=0;
	rotz[1][1]=1;
	rotz[1][2]=0;

	rotz[2][0]=sin(rot);
	rotz[2][1]=0;
	rotz[2][2]=cos(rot);
	//Calculate rotate cube
	for(i=0;i<8;i++){
		float nx,ny,nz;
		nx=rotz[0][0]*cube[i].x + rotz[1][0]*cube[i].y + rotz[2][0]*cube[i].z;
		ny=rotz[0][1]*cube[i].x + rotz[1][1]*cube[i].y + rotz[2][1]*cube[i].z;
		nz=rotz[0][2]*cube[i].x + rotz[1][2]*cube[i].y + rotz[2][2]*cube[i].z;
		cube[i].x=nx;
		cube[i].y=ny;
		cube[i].z=nz;
	}

	ofstream myfile ("/tmp/test_002.data");
	myfile << "#box\n";
	//Write file to gnuplot
	myfile << cube[0].x << "\t" << cube[0].y << "\t" << cube[0].z << "\n";
	myfile << cube[1].x << "\t" << cube[1].y << "\t" << cube[1].z << "\n";
	myfile << cube[3].x << "\t" << cube[3].y << "\t" << cube[3].z << "\n";
	myfile << cube[2].x << "\t" << cube[2].y << "\t" << cube[2].z << "\n";
	myfile << cube[0].x << "\t" << cube[0].y << "\t" << cube[0].z << "\n";
	myfile << cube[4].x << "\t" << cube[4].y << "\t" << cube[4].z << "\n";
	myfile << cube[6].x << "\t" << cube[6].y << "\t" << cube[6].z << "\n";
	myfile << cube[2].x << "\t" << cube[2].y << "\t" << cube[2].z << "\n";
	myfile << cube[3].x << "\t" << cube[3].y << "\t" << cube[3].z << "\n";
	myfile << cube[7].x << "\t" << cube[7].y << "\t" << cube[7].z << "\n";
	myfile << cube[6].x << "\t" << cube[6].y << "\t" << cube[6].z << "\n";
	myfile << cube[4].x << "\t" << cube[4].y << "\t" << cube[4].z << "\n";
	myfile << cube[5].x << "\t" << cube[5].y << "\t" << cube[5].z << "\n";
	myfile << cube[7].x << "\t" << cube[7].y << "\t" << cube[7].z << "\n";
	myfile << cube[3].x << "\t" << cube[3].y << "\t" << cube[3].z << "\n";
	myfile << cube[1].x << "\t" << cube[1].y << "\t" << cube[1].z << "\n";
	myfile << cube[5].x << "\t" << cube[5].y << "\t" << cube[5].z << "\n";

	//Camera 
	myfile << "\n\n#Camera\n";
	myfile << "0\t0\t" << (focal*-2) << "\n";

	//Image Plane
	myfile << "\n\n#Image Plane\n";
	myfile << "-2\t1.5\t" << (focal*-1) << "\n";
	myfile << "2\t1.5\t"  << (focal*-1) << "\n";
	myfile << "2\t-1.5\t" << (focal*-1) << "\n";
	myfile << "-2\t-1.5\t" << (focal*-1) << "\n";
	myfile << "-2 \t1.5 \t" << (focal*-1) << "\n";
	
	//Camera Rays
	myfile << "\n\n#Camera Rays";
	for( i=0; i<8; i++){
		myfile << "0\t0\t" << (focal*-2) << "\n";
		myfile << cube[i].x << "\t" << cube[i].y << "\t" << cube[i].z << "\n";
		myfile << "0\t0\t" << (focal*-2) << "\n";
	}

	//Calculate 3d to 2d
	myfile << "\n\n#2d image of 3d\n";
	for( i=0; i<8; i++){
		imageCube[i].x=cube[i].x*focal/(cube[i].z+2*focal);
		imageCube[i].y=cube[i].y*focal/(cube[i].z+2*focal);
		myfile << imageCube[i].x << "\t" << imageCube[i].y << "\t"<<(-1*focal)<<"\n";
	}
	
	myfile.close();	

	FILE *pg = popen("/usr/local/bin/gnuplot", "w");
	if(!pg)
	{
		cerr << "popen failed: " << strerror(errno) << '\n';
		return 1;
	}
	//fputs("plot  \"/tmp/test_001.data\" index 1 title \"Points\" with vectors, \"/tmp/test_001.data\" index 2 title \"Intersections\", \"/tmp/test_001.data\" index 0 title \"Vanishing point\"  \n", pg);
	fputs("set terminal x11\n", pg);
	fflush(pg);
	fputs("set xrange[-10:10]\n", pg);
	fflush(pg);
	fputs("set yrange[-10:10]\n", pg);
	fflush(pg);
	fputs("set zrange[-10:10]\n", pg);
	fflush(pg);
	fputs("splot '/tmp/test_002.data' index 0 title 'Box'  w l, '/tmp/test_002.data' index 1 title 'Camera' , '/tmp/test_002.data' index 2 title 'Image Window'  w l, '/tmp/test_002.data' index 3 title 'Camera Rays' w l, '/tmp/test_002.data' index 4 title 'Image Box' \n", pg);
	fflush(pg);
	
	cout << "Write q to exit\n";
	string str;
	cin >> str;
	while(str!="q")
		cin >> str;

	pclose(pg);	
}

